#include "Reader.h"

#define VDRIVE_ENABLE

void extractArgs(char code_line[codeMax],FLOAT_I floatArg[9],int num)
{
    int argPtr = 2;
    char args[15];
    for(int i=0;i<num;i++)
    {
        int j;            
        for(j=0;ISDIGIT(code_line[argPtr])|| code_line[argPtr]=='+' || code_line[argPtr]=='-' || code_line[argPtr]=='.';j++)
        {
          args[j] = code_line[argPtr++];         
          if(code_line[argPtr] == '#')
          {
            break;
          }
        }
        args[j] = '\0';
        floatArg[i].str2floati(args);        
        argPtr++;
    }
}

/*
void concatenate(char* source,char* dest)
{
  int i,str_length = 0; 
  for(i=0;source[i]!='\0';i++)
  {
    str_length = str_length + 1;
  }
  i=0;
  while(dest[i]!='\0')
  {
    source[str_length + i] = dest[i];
    i++;
  }
  source[str_length] = '\0';
}
*/
/*
void IntToStr(int a, char* str)
{
	int flag = 1;
	if(a<0)
		flag = 0;
	if(flag==0)
		a = -a;
	int i = 0;
	while(a>0)
	{
		str[i] = char(a%10 + '0');
		i++;
		a = a/10;
	}
	if(flag == 0)
	{
		str[i] = '-';
		str[i+1] = '\0';
	}
	else
		str[i] = '\0';
	int length;
	for(length=1;str[length]!='\0';length++);

	char ch;
	for(int k=0;k<length/2;k++)
	{
		ch = str[k];
		str[k] = str[length-k-1];
		str[length-k-1] = ch;
	}
	
	str[length] = '\0';
}
*/
/*
void pulse(int a,int b)
{
  digitalWrite(13,1);
  delay(a);
  digitalWrite(13,0);
  delay(b);
}
*/

void parseLine(char code_line[codeMax])
{
        int i;
	char args[15];
	FLOAT_I floatArg[9];
	int32 int_args[9];
	char funCode;
	int argPtr = 2;
        funCode = code_line[0];
	switch(funCode)
	{
		case 65 + 0://A SET_ORIGIN_OFFSETS(double x, double y, double z, double a, double b, double c)              
			//out<<"\nA";
			//concatenate(op,"\nA");
			extractArgs(code_line,floatArg,6);
			for(int i=0;i<3;i++)
			{   
				if(currUnit == CANON_UNITS_CM)
					int_args[i] = floatArg[i].floatcm_to_micron();
				else if(currUnit == CANON_UNITS_MM)
					int_args[i] = floatArg[i].floatmm_to_micron();
				else if(currUnit == CANON_UNITS_INCHES)
					int_args[i] = floatArg[i].floatinch_to_micron();
				//out<<" "<<int_args[i];
				//concatenate(op," ");
				//IntToStr(int_args[i],c);
				//concatenate(op,c);                    
			}
          		//loop converts the values in degrees of last 3 axes to milli degrees
			for(int i=3;i<6;i++)
			  int_args[i] = floatArg[i].floatmm_to_micron();
                        SET_ORIGIN_OFFSETS(int_args[0], int_args[1], int_args[2], int_args[3], int_args[4], int_args[5]);
                        //SerialUSB.println(code_line);
			break;
		case 65 + 1://B USE_LENGTH_UNITS(CANON_UNITS units)
			//out<<"\nB";                  
			//concatenate(op,"\nB");                              
			USE_LENGTH_UNITS(TODIGIT(code_line[argPtr]));
			//out<<" "<<currUnit;
			//concatenate(op," ");
			//IntToStr(currUnit,c);
			//concatenate(op,c);
			//SerialUSB.println(code_line);
			break;
		case 65 + 2://C STRAIGHT_TRAVERSE(double x, double y, double z, double a, double b, double c);
			//out<<"\nC";
			//concatenate(op,"\nC");
			extractArgs(code_line,floatArg,6);
			for(int i=0;i<3;i++)
			{
				if(currUnit == CANON_UNITS_CM)
					int_args[i] = floatArg[i].floatcm_to_micron();
				else if(currUnit == CANON_UNITS_MM)
					int_args[i] = floatArg[i].floatmm_to_micron();
				else if(currUnit == CANON_UNITS_INCHES)
					int_args[i] = floatArg[i].floatinch_to_micron();
				
				//out<<" "<<int_args[i];
				//concatenate(op," ");
				//IntToStr(int_args[i],c);
				//concatenate(op,c);
			}
			
			//loop converts the values in degrees of last 3 axes to milli degrees
			for(int i=3;i<6;i++)
			  int_args[i] = floatArg[i].floatmm_to_micron();
			
			STRAIGHT_TRAVERSE(int_args[0], int_args[1], int_args[2], int_args[3], int_args[4], int_args[5]); 
			//SerialUSB.println(code_line);
			break;
		case 65 + 3://D SELECT_PLANE(CANON_PLANE plane)
			//out<<"\nD";
			//concatenate(op,"\nD");
			SELECT_PLANE(TODIGIT(code_line[argPtr]));
			//out<<" "<<currPlane;
			//SerialUSB.println(code_line);
			break;
		case 65 + 4://E SET_FEED_RATE(double rate)
			//feed rate mode not implemented
			//out<<"\nE";
			//concatenate(op,"\nE");
			extractArgs(code_line,floatArg,1);
			if(currUnit == CANON_UNITS_CM)
					int_args[0] = floatArg[0].floatcm_to_micron();
			else if(currUnit == CANON_UNITS_MM)
					int_args[0] = floatArg[0].floatmm_to_micron();
			else if(currUnit == CANON_UNITS_INCHES)
					int_args[0] = floatArg[0].floatinch_to_micron();
				
			//out<<" "<<int_args[0];
			int_args[0] = int_args[0] / 60;  //converted from unit per minute to unit per second
			SET_FEED_RATE(int_args[0]);
			//SerialUSB.println(code_line);
			break;
		case 65 + 5://F SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference)
			//feed reference canon_xyz not clear
			//out<<"\nF";
			//concatenate(op,"\nF");
			SET_FEED_REFERENCE(TODIGIT(code_line[argPtr]));
			//out<<" "<<curreFeedRef;
			//SerialUSB.println(code_line);
			break;
		case 65 + 6://G SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode)
			//out<<"\nG";
			//concatenate(op,"\nG");
			SET_MOTION_CONTROL_MODE(TODIGIT(code_line[argPtr]));
			//out<<" "<<currMotionMode;
			break;
		case 65 + 7://H START_SPEED_FEED_SYNCH()
			//out<<"\nH";
			//concatenate(op,"\nH");
			START_SPEED_FEED_SYNCH();
			break;
		case 65 + 8://I STOP_SPEED_FEED_SYNCH()
			//out<<"\nI";
			//concatenate(op,"\nI");  
			STOP_SPEED_FEED_SYNCH();
			break;
		case 65 + 9://J ARC_FEED(double first_end, double second_end, double first_axis, double second_axis, int rotation, double axis_end_point, double a, double b, double c)
			//out<<"\nJ";
			//concatenate(op,"\nJ");
			extractArgs(code_line,floatArg,9);
			for(int i=0;i<4;i++)
			{
				if(currUnit == CANON_UNITS_CM)
					int_args[i] = floatArg[i].floatcm_to_micron();
				else if(currUnit == CANON_UNITS_MM)
					int_args[i] = floatArg[i].floatmm_to_micron();
				else if(currUnit == CANON_UNITS_INCHES)
					int_args[i] = floatArg[i].floatinch_to_micron();
				
				//out<<" "<<int_args[i];
				argPtr++;
			}
			int_args[4] = floatArg[4].toInt();
			//out<<" "<<int_args[4];
			for(int i=5;i<9;i++)
			{
				if(currUnit == CANON_UNITS_CM)
					int_args[i] = floatArg[i].floatcm_to_micron();
				else if(currUnit == CANON_UNITS_MM)
					int_args[i] = floatArg[i].floatmm_to_micron();
				else if(currUnit == CANON_UNITS_INCHES)
					int_args[i] = floatArg[i].floatinch_to_micron();
				
				//out<<" "<<int_args[i];
				argPtr++;
			}
			ARC_FEED(int_args[0], int_args[1], int_args[2], int_args[3], int_args[4], int_args[5], int_args[6], int_args[7], int_args[8]);
			break;
		case 65 + 10://K DWELL(double milliseconds) dwell modified to take input in milliseconds
			//out<<"\nK";
			extractArgs(code_line,floatArg,1);
			int_args[0] = floatArg[0].floatmm_to_micron();  //same as doing sec to millisecond
			
			//out<<" "<<int_args[0];
			DWELL(int_args[0]);
			break;
		case 65 + 11://L STRAIGHT_FEED(double x, double y, double z, double a, double b, double c)
			//out<<"\nL";
			extractArgs(code_line,floatArg,6);
			for(int i=0;i<3;i++)
			{
				if(currUnit == CANON_UNITS_CM)
					int_args[i] = floatArg[i].floatcm_to_micron();
				else if(currUnit == CANON_UNITS_MM)
					int_args[i] = floatArg[i].floatmm_to_micron();
				else if(currUnit == CANON_UNITS_INCHES)
					int_args[i] = floatArg[i].floatinch_to_micron();
				//out<<" "<<int_args[i];
				//SerialUSB.println(int_args[i]);
			}
			//loop converts the values in degrees of last 3 axes to milli degrees
			for(int i=3;i<6;i++)
			{
			  int_args[i] = floatArg[i].floatmm_to_micron();
			  //SerialUSB.println(int_args[i]);
			}
			
			STRAIGHT_FEED(int_args[0], int_args[1], int_args[2], int_args[3], int_args[4], int_args[5]);
			//SerialUSB.println(code_line);
			break;                
		case 65 + 12://M STRAIGHT_PROBE(double x, double y, double z, double a, double b, double c)
			//Not to be implemented in current version
			/*TURN_PROBE_ON()
			  M 3.0000 2.0000 3.0000 0.0000 0.0000 0.0000 #STRAIGHT_PROBE
			  TURN_PROBE_OFF()*/
			//out<<"\nM";
			extractArgs(code_line,floatArg,6);
			for(int i=0;i<3;i++)
			{
				if(currUnit == CANON_UNITS_CM)
					int_args[i] = floatArg[i].floatcm_to_micron();
				else if(currUnit == CANON_UNITS_MM)
					int_args[i] = floatArg[i].floatmm_to_micron();
				else if(currUnit == CANON_UNITS_INCHES)
					int_args[i] = floatArg[i].floatinch_to_micron();
				
				//out<<" "<<int_args[i];
			}
			//loop converts the values in degrees of last 3 axes to milli degrees
			for(int i=3;i<6;i++)
			  int_args[i] = floatArg[i].floatmm_to_micron();
			//TURN_PROBE_ON()
			//STRAIGHT_PROBE(int_args[0], int_args[1], int_args[2], int_args[3], int_args[4], int_args[5], int_args[6]);
			//TURN_PROBE_OFF();
			break;
		case 65 + 13://N ORIENT_SPINDLE(double orientation, CANON_DIRECTION direction)
			//Not to be implemented in current version
			//out<<"\nN";
			extractArgs(code_line,floatArg,1);
			int_args[0] = floatArg[0].floatmm_to_micron();  //degree converted to milli degree
			
			direction = TODIGIT(code_line[++argPtr]);
			//out<<" "<<int_args[0]<<" "<<direction;
			//ORIENT_SPINDLE(int_args[0], direction);
			break;
		case 65 + 14://O SET_SPINDLE_SPEED(double r)
			//out<<"\nO";
			extractArgs(code_line,floatArg,1);
							
			int_args[0] = floatArg[0].floatmm_to_micron();  //same as doing rpm to millirpm
			
			//out<<" "<<int_args[0];
			SET_SPINDLE_SPEED(int_args[0]);
			break;
		case 65 + 15://P START_SPINDLE_CLOCKWISE()
			//out<<"\nP";
			START_SPINDLE_CLOCKWISE();
			break;
		case 65 + 16://Q START_SPINDLE_COUNTERCLOCKWISE()
			//out<<"\nQ";
			START_SPINDLE_COUNTERCLOCKWISE();
			break;
		case 65 + 17://R STOP_SPINDLE_TURNING()
			//out<<"\nR";
			STOP_SPINDLE_TURNING();
			break;
		case 65 + 18://S CHANGE_TOOL(int slot)
			//not implemented in this version
			//out<<"\nS";
			extractArgs(code_line,floatArg,1);
			
			int_args[0] = floatArg[0].toInt();
			//out<<" "<<int_args[0];
			CHANGE_TOOL(int_args[0]);
			break;
		case 65 + 19://T SELECT_TOOL(int i)
			//not implemented in this verison
			//out<<"\nT";
			extractArgs(code_line,floatArg,1);
			
			int_args[0] = floatArg[0].toInt();
			//out<<" "<<int_args[0];
			//SELECT_TOOL(int_args[0]);
			break;
		case 65 + 20://U USE_TOOL_LENGTH_OFFSET(double offset)
			//out<<"\nU";
			extractArgs(code_line,floatArg,1);
			
			if(currUnit == CANON_UNITS_CM)
					int_args[0] = floatArg[0].floatcm_to_micron();
			else if(currUnit == CANON_UNITS_MM)
				int_args[0] = floatArg[0].floatmm_to_micron();
			else if(currUnit == CANON_UNITS_INCHES)
				int_args[0] = floatArg[0].floatinch_to_micron();

			//out<<" "<<int_args[0];
			USE_TOOL_LENGTH_OFFSET(int_args[0]);
			break;
		case 65 + 21://V COMMENT(char * s)
			//out<<"\nV";
			argPtr = 3;
			for(i=0;code_line[argPtr]!='"';i++)
				args[i] = code_line[argPtr++];
			args[i] = '\0';
			//out<<" "<<args;
			//COMMENT(args);
			break;
		case 65 + 22://W DISABLE_FEED_OVERRIDE()
			//out<<"\nW";
			//call DISABLE_FEED_OVERRIDE();
			break;
		case 65 + 23://X DISABLE_SPEED_OVERRIDE()
			//out<<"\nX";
			//call DISABLE_SPEED_OVERRIDE()
			break;
		case 65 + 24://Y ENABLE_FEED_OVERRIDE()
			//out<<"\nY";
			//call ENABLE_FEED_OVERRIDE()
			break;
		case 65 + 25://Z ENABLE_SPEED_OVERRIDE()
			//out<<"\nZ";
			//call ENABLE_SPEED_OVERRIDE()
			break;
		case 65 + 26://[ FLOOD_OFF()
			//out<<"\n[";
			FLOOD_OFF();
			break;
		case 65 + 27://\ FLOOD_ON()
			//out<<"\n\\";
			FLOOD_ON();
			break;
		case 65 + 28://] INIT_CANON()
			//out<<"\n]";
			//call INIT_CANON()
			break;
		case 65 + 29://^ MESSAGE(char * s)
			//out<<"\n^";
			//not implemented in this verison
			argPtr = 3;
			for(i=0;code_line[argPtr]!='"';i++)
				args[i] = code_line[argPtr++];
			args[i] = '\0';
			//out<<" "<<args;
			//MESSAGE(args);
			break;
		case 65 + 30://_ MIST_OFF()
			//out<<"\n_";
			MIST_OFF();
			break;
		case 65 + 31://` MIST_ON()
			//out<<"\n`";
			MIST_ON();
			break;
		case 65 + 32://a PALLET_SHUTTLE()
			//not implemented in this verison
			//out<<"\na";
			//call PALLET_SHUTTLE()
			break;
		case 65 + 33://b OPTIONAL_PROGRAM_STOP()
			//out<<"\nb";
			//call OPTIONAL_PROGRAM_STOP()
			break;
		case 65 + 34://c PROGRAM_END()
			//out<<"\nc";
			//call PROGRAM_END()
			break;
		case 65 + 35://d PROGRAM_STOP()
			//out<<"\nd";
			//call PROGRAM_STOP()
			break;                      
		default:
			SerialUSB.println("default");
	}
}

#ifdef VDRIVE_ENABLE
//reader with codeline as input
void reader(char *code_line)
{
  SerialUSB.println(code_line);
   parseLine(code_line);
}

#endif


//reader with file as input
#ifndef VDRIVE_ENABLE
void reader(char *fileName)
{
  SerialUSB.println("in reader without vdrive");
    int codePtr = 0;
  //  vdr.init();

//    char ch[] = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 1.0000 #SET_FEED_RATE\nL 50.000 -50.000 0.000 0.000 0.000 0.000 #STRAIGHT_FEED\n";
//    char ch[] = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 1.0000 #SET_FEED_RATE\nL -25.000 0.000 -25.000 0.000 0.000 0.000 #STRAIGHT_FEED\nC 0.000 0.000 0.000 0.000 0.000 0.000 #STRAIGHT_FEED\n";

//    char ch[] = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 5.0000 #SET_FEED_RATE\nL 0.0000 -50.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nJ -50.0000 0.0000 0.0000 0.0000 -1 0.0000 0.0000 0.0000 0.0000 #ARC_FEED\nC 0.000 0.000 0.000 0.000 0.000 0.000 #straight_traverse\n";
//    char ch[] = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 3.0000 #SET_FEED_RATE\nL 0.0000 25.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nJ -25.0000 0.0000 0.0000 0.0000 1 0.0000 0.0000 0.0000 0.0000 #ARC_FEED\n";

//pawn
//  char ch[] = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 2.0000 #SET_FEED_RATE\nD 3 #SELECT_PLANE\nJ 5.000 -5.000 5.0000 0.0000 -1 0.0000 0.0000 0.0000 0.0000 #ARC_FEED\nL -3.0000 0.0000 7.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -5.0000 0.0000 9.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -3.0000 0.0000 11.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -7.5.0000 0.0000 25.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nC -9.0000 0.0000 25.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\nC 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\n";
//  char ch[] = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 2.0000 #SET_FEED_RATE\nD 3 #SELECT_PLANE\nJ 0.005 -0.005 0.0050 0.0000 1 0.0000 0.0000 0.0000 0.0000 #ARC_FEED\nL -3.0000 0.0000 7.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -5.0000 0.0000 9.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -0.0030 0.0000 11.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -7.5.0000 0.0000 25.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nC -9.0000 0.0000 25.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\nC 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\n";
//bishop
//    char ch[]  = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 0.1500 #SET_FEED_RATE\nL -5.0000 0.0000 5.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -3.0000 0.0000 7.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -5.0000 0.0000 10.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -3.0000 0.0000 12.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -9.5000 0.0000 25.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -6.0000 0.0000 50.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -10.0000 0.0000 60.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nC -11.0000 0.0000 60.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\nC -11.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\nC 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\n";
    char ch[]  = "B 2 #USE_LENGTH_UNITS\nA 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #SET_ORIGIN_OFFSETS\nF 2 #SET_FEED_REFERENCE\nE 2.0000 #SET_FEED_RATE\nL -5.0000 0.0000 5.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -3.0000 0.0000 7.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -5.0000 0.0000 10.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -3.0000 0.0000 12.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -9.5000 0.0000 25.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -6.0000 0.0000 50.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nL -10.0000 0.0000 60.0000 0.0000 0.0000 0.0000 #STRAIGHT_FEED\nC -11.0000 0.0000 60.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\nC -11.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\nC 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 #STRAIGHT_TRAVERSE\n";

    //  ch = vdr.fread(fileName);
    char code_line[codeMax];
    while(ch[codePtr]!='\0')
    { 
      for(int i=0;i<codeMax;i++)      // reading line
      {
            if(ch[codePtr] == EOL)
            {
                code_line[i] = '\0';
                while(ch[codePtr++]!='\n')
                    ;
                break;
            }
            code_line[i] = ch[codePtr];
            codePtr++; 
       } 
      parseLine(code_line);
      SerialUSB.println(code_line);        
  }
  //vdr.fopen("output10.txt");
  //vdr.fputs(op);
  //vdr.fclose("output10.txt");
}

#endif
